#include "icm20602.h"
#include <stdio.h>

/**
 * @brief SPI外设初始化
 */
void ICM20602_GPIO_SPI_Initialization(void)
{
	CHIP_SELECT(ICM20602);

	LL_SPI_Enable(ICM20602_SPI_CHANNEL);

	CHIP_DESELECT(ICM20602);
}

/**
 * @brief 使用SPI写/读一字节数据
 * @param  data             发送的数据
 * @return unsigned char 
 */
unsigned char SPI_Write_Read_Byte(unsigned char data)
{
	while(LL_SPI_IsActiveFlag_TXE(ICM20602_SPI_CHANNEL)==RESET);
	LL_SPI_TransmitData8(ICM20602_SPI_CHANNEL, data);
	
	while(LL_SPI_IsActiveFlag_RXNE(ICM20602_SPI_CHANNEL)==RESET);
	return LL_SPI_ReceiveData8(ICM20602_SPI_CHANNEL);
}

/**
 * @brief 从ICM20602中读取一字节
 * @param  reg_addr         寄存器地址
 * @return uint8_t 
 */
uint8_t ICM20602_Readbyte(uint8_t reg_addr)
{
	CHIP_SELECT(ICM20602);
	SPI_Write_Read_Byte(reg_addr | 0x80); // 将最高位设置为1，表示读指令
	uint8_t val = SPI_Write_Read_Byte(0x00);
	CHIP_DESELECT(ICM20602);
	
	return val;
}

/**
 * @brief 从ICM20602中读取多个字节
 * @param  reg_addr         寄存器地址
 * @param  len             	数据长度
 * @param  data             数据缓存区指针
 */
void ICM20602_Readbytes(unsigned char reg_addr, unsigned char len, unsigned char* data)
{
	unsigned int i = 0;

	CHIP_SELECT(ICM20602);
	SPI_Write_Read_Byte(reg_addr | 0x80); // 将最高位设置为1，表示读指令
	for(int i = 0; i < len; i++)
		data[i] = SPI_Write_Read_Byte(0x00);
	CHIP_DESELECT(ICM20602);
}

/**
 * @brief 向ICM20602写入1字节数据
 * @param  reg_addr         寄存器地址
 * @param  val              写入的数据
 */
void ICM20602_Writebyte(uint8_t reg_addr, uint8_t val)
{
	CHIP_SELECT(ICM20602);
	SPI_Write_Read_Byte(reg_addr & 0x7F); // 将最高位设置为0，表示写指令
	SPI_Write_Read_Byte(val);
	CHIP_DESELECT(ICM20602);
}

/**
 * @brief 向ICM20602写入多个字节数据
 * @param  reg_addr         寄存器地址
 * @param  len              写入的数据
 * @param  data             待写入数据的指针
 */
void ICM20602_Writebytes(unsigned char reg_addr, unsigned char len, unsigned char* data)
{
	CHIP_SELECT(ICM20602);
	SPI_Write_Read_Byte(reg_addr & 0x7F); // 将最高位设置为0，表示写指令
	for(int i = 0; i < len; i++)
		SPI_Write_Read_Byte(data[i]);
	CHIP_DESELECT(ICM20602);
}

/**
 * @brief ICM20602初始化
 * @return int 
 */
int ICM20602_Initialization(void)
{
	ICM20602_GPIO_SPI_Initialization();
	
	printf("Checking ICM20602...");
	
	// check WHO_AM_I (0x75)
	uint8_t who_am_i = ICM20602_Readbyte(WHO_AM_I); 
	
	// who am i = 0x12
	if(who_am_i == 0x12)
	{
		printf("\nICM20602 who_am_i = 0x%02x...OK\n\n", who_am_i);
	}
	// recheck
	else if(who_am_i != 0x12)
	{
		who_am_i = ICM20602_Readbyte(WHO_AM_I); // check again WHO_AM_I (0x75)

		if (who_am_i != 0x12){
			printf( "ICM20602 Not OK: 0x%02x Should be 0x%02x\n", who_am_i, 0x12);
			return 1; //ERROR
		}
	}
	
	// 复位ICM20602
	ICM20602_Writebyte(PWR_MGMT_1, 0x80);
	HAL_Delay(50);

	// 使能温度传感器
	ICM20602_Writebyte(PWR_MGMT_1, 0x01);
	HAL_Delay(50);

	// 使能加速度计和陀螺仪
	ICM20602_Writebyte( PWR_MGMT_2, 0x00 );
	HAL_Delay(50);
	
	// 设置采样率为1000Hz，同时设置软件滤波
	ICM20602_Writebyte(SMPLRT_DIV, 0x00);
	HAL_Delay(50);
	
	// 设置陀螺仪低通滤波的截止频率为20Hz
	ICM20602_Writebyte(CONFIG, 0x05);
	HAL_Delay(50);

	// 设置加速度计低通滤波的截止频率为44.8Hz
	ICM20602_Writebyte(ACCEL_CONFIG2, 0x03);
	HAL_Delay(50);

	// 设置陀螺仪量程为±2000deg/s
	ICM20602_Writebyte(GYRO_CONFIG, 0x18);
	HAL_Delay(50);

	// 设置加速度计量程为±16g
	ICM20602_Writebyte(ACCEL_CONFIG, 0x18);
	HAL_Delay(50);
	
	// 使能数据准备完毕中断，即INT
	ICM20602_Writebyte(INT_ENABLE, 0x01);
	HAL_Delay(50);
	
	return 0; //OK
}

/**
 * @brief 读取ICM20602六轴原始数据
 * @param  accel            加速度原始数据缓存区指针
 * @param  gyro             角速度原始数据缓存区指针
 */
void ICM20602_Get6AxisRawData(short *accel, short *gyro)
{
	unsigned char data[14];
	ICM20602_Readbytes(ACCEL_XOUT_H, 14, data);
	
	accel[0] = (data[0] << 8) | data[1];
	accel[1] = (data[2] << 8) | data[3];
	accel[2] = (data[4] << 8) | data[5];

	gyro[0] = ((data[8] << 8) | data[9]);
	gyro[1] = ((data[10] << 8) | data[11]);
	gyro[2] = ((data[12] << 8) | data[13]);
	
}

#include <math.h>
#include "imu.h"
sensor_st sensor;
int imu_data_ready;
float euler[3] = {0.0f, 0.0f, 0.0f};
float gyro_offsets_sum[3]= { 0.0f, 0.0f, 0.0f };
float gyroOffset[3]= { 0.0f, 0.0f, 0.0f };
float accel_offsets_sum[3] = { 0.0f, 0.0f, 0.0f };
float accelOffset[3]= { 0.0f, 0.0f, 0.0f };
float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };
int offset_count = 0;
uint8_t ready = 0;
float accb[3] = { 0.0f, 0.0f, 0.0f };
/**
 * @brief INT中断回调函数
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == GPIO_PIN_12)
	{
		// 读取原始数据
		ICM20602_Get6AxisRawData(sensor.accel_raw, sensor.gyro_raw);
		for(int i = 0; i < 3; i++)
		{
			// 陀螺仪原始数据转换为deg/s
			sensor.gyro_dps[i] = sensor.gyro_raw[i] / 65536.0f * 4000;
			// 陀螺仪原始数据转换为rad/s
			sensor.gyro_rps[i] = sensor.gyro_dps[i] * 3.1415926535f / 180.0f;
			// 加速度计原始数据转换为m/s
			sensor.accel_mps[i] = sensor.accel_raw[i] / 65536.0f * 32 * CONSTANTS_ONE_G;
		}		
			
		if(!ready)
		{
				gyro_offsets_sum[0] += sensor.gyro_rps[0];
				gyro_offsets_sum[1] += sensor.gyro_rps[1];
				gyro_offsets_sum[2] += sensor.gyro_rps[2];
				accel_offsets_sum[0] += sensor.accel_mps[0];
				accel_offsets_sum[1] += sensor.accel_mps[1];
				accel_offsets_sum[2] += sensor.accel_mps[2];
				offset_count++;
				if(offset_count >= 3000)
				{
						gyroOffset[0] = gyro_offsets_sum[0] / offset_count;
						gyroOffset[1] = gyro_offsets_sum[1] / offset_count;
						gyroOffset[2] = gyro_offsets_sum[2] / offset_count;
						accelOffset[0] = accel_offsets_sum[0]/offset_count;
						accelOffset[1] = accel_offsets_sum[1]/offset_count;
						accelOffset[2] = accel_offsets_sum[2]/offset_count;
						accelOffset[2]-= CONSTANTS_ONE_G;
						offset_count=0;
						gyro_offsets_sum[0]=0;
						gyro_offsets_sum[1]=0;
						gyro_offsets_sum[2]=0;
						ready = 1;
				}
				return;
		}

		sensor.gyro_rps[0] -= gyroOffset[0];
		sensor.gyro_rps[1] -= gyroOffset[1];
		sensor.gyro_rps[2] -= gyroOffset[2];

		sensor.accel_mps[0] -= accelOffset[0];
		sensor.accel_mps[1] -= accelOffset[1];
		sensor.accel_mps[2] -= accelOffset[2];
		
		MahonyUpdate(sensor.gyro_rps[0], sensor.gyro_rps[1], sensor.gyro_rps[2], 
											sensor.accel_mps[0], sensor.accel_mps[1], sensor.accel_mps[2]);
		imu_data_ready = 1;

		Rot_matrix[0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;// 11
		Rot_matrix[1] = 2.f * (q1*q2 + q0 * q3);	// 12
		Rot_matrix[2] = 2.f * (q1*q3 - q0 * q2);	// 13
		Rot_matrix[3] = 2.f * (q1*q2 - q0 * q3);	// 21
		Rot_matrix[4] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3;// 22
		Rot_matrix[5] = 2.f * (q2 * q3 + q0 * q1);	// 23
		Rot_matrix[6] = 2.f * (q1 * q3 + q0 * q2);	// 31
		Rot_matrix[7] = 2.f * (q2 * q3 - q0 * q1);	// 32
		Rot_matrix[8] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;// 33
		
		sensor.rol = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
		sensor.pit = -asinf(Rot_matrix[2]);									//! Pitch
		sensor.yaw = atan2f(Rot_matrix[1], Rot_matrix[0]);
	}
}
